14. 卡尔曼方程参考

卡尔曼方程参考

我们把卡尔曼滤波器方程列在这里，供你随时参考。请继续 :)

变量定义

x^ \mathbf{\hat{x}} - 状态向量

F \mathbf{F} - 状态转移矩阵

P \mathbf{P} - 误差协方差矩阵

Q \mathbf{Q} - 过程噪声协方差矩阵

R \mathbf{R} - 测量噪音协方差矩阵

S \mathbf{S} - 用于计算卡尔曼增益的中间矩阵

K \mathbf{K} - 卡尔曼增益

y~ \mathbf{\tilde{y}} - 预测状态和测量状态之差

z \mathbf{z} - 测量向量（激光雷达数据、雷达数据等）

I \mathbf{I} - 标识矩阵

预测步骤方程

预测态矢量和误差协方差矩阵

x^k∣k−1=Fkx^k−1∣k−1 \mathbf{\hat{x} {k|k-1}} = \mathbf{F {k}} \mathbf{\hat{x}_{k-1|k-1}}

Pk∣k−1=FkPk−1∣k−1FkT+Qk \mathbf{P_{k|k-1}} = \mathbf{F_{k}} \mathbf{P_{k-1|k-1}} \mathbf{F_{k}^T} + \mathbf{Q_{k}}

更新步骤方程

卡尔曼增益

Sk=HkPk∣k−1HkT+Rk \mathbf{S_{k}} = \mathbf{H_{k}} \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} + \mathbf{R_{k}}

Kk=Pk∣k−1HkTSk−1 \mathbf{K_{k}} = \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} \mathbf{S_{k}}^{-1}

更新状态向量和错误协调矩阵

yk~=zk−Hkx^k∣k−1 \mathbf{\tilde{y_{k}}} = \mathbf{z_{k}} - \mathbf{H_{k}} \mathbf{\hat{x}_{k|k-1}}

x^k∣k=x^k∣k−1+Kkyk~ \mathbf{\hat{x} {k|k}} = \mathbf{\hat{x} {k|k-1}} +\mathbf{ K_{k}} \mathbf{\tilde{y_{k}}}

Pk∣k=(I−KkHk)Pk∣k−1 \mathbf{P_{k|k}} = (\mathbf{I} - \mathbf{ K_{k}} \mathbf{H_{k}}) \mathbf{P_{k|k-1}}

Next Concept